// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ATIOmega85.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Hardware.Sensors.ATI
{
    using System;
    using System.IO;
    using System.Net;
    using System.Net.Sockets;
    using System.Threading;
    using System.Xml;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// A class to interact with an ATI Omega85 force torque sensor
    /// Reference can be found at the following url
    /// </summary>
    /// https://microsoft.sharepoint.com/teams/Baker/Shared%20Documents/Reference%20documents/Hardware%20Manual/ATI/9610-05-1022%20Manual.pdf
    public class ATIOmega85 : IDisposable
    {
        /// <summary>
        /// The port on the net box through which it is broadcasting
        /// </summary>
        private const int ATIPort = 49152;

        /// <summary>
        /// Context for the sensor monitor
        /// </summary>
        private static TraceContext sensorMonitorContext = TraceContexts.FromString(TraceContexts.HW, "ATIOmega85");

        /// <summary>
        /// The request data
        /// </summary>
        private byte[] request;
        
        /// <summary>
        /// The UDP Client
        /// </summary>
        private UdpClient udpClient;

        /// <summary>
        /// The IP endpoint
        /// </summary>
        private IPEndPoint endPoint;

        /// <summary>
        /// Most recent bytes received from the sensor
        /// </summary>
        private volatile byte[] mostRecentlyReceivedBytes;

        /// <summary>
        /// IP address of the sensor
        /// </summary>
        private string ipAddress;

        /// <summary>
        /// Counts per unit force reading
        /// </summary>
        private int countPerForce;

        /// <summary>
        /// Counts per unit torque reading
        /// </summary>
        private int countPerTorque;

        /// <summary>
        /// Boolean indicating whether force is measured in Newton
        /// </summary>
        private bool isForceMeasuredInNewton;

        /// <summary>
        /// Boolean indicating whether torque is measured in Newton-Meter
        /// </summary>
        private bool isTorqueMeasuredInNewtonMeter;

        /// <summary>
        /// Boolean indicating whether the current sensor configuration is obtained
        /// </summary>
        private bool configurationObtained;

        /// <summary>
        /// A spinlock to guard access to the udp client
        /// Mainly used for the disposal process
        /// </summary>
        private SpinLock spinLock;

        /// <summary>
        /// Indicates whether or not instance has been disposed.
        /// </summary>
        private bool disposed;

        /// <summary>
        /// Gets the most recent force information
        /// </summary>
        private Vector3 mostRecentForce;

        /// <summary>
        /// Gets the most recent torque information
        /// </summary>
        private Vector3 mostRecentTorque;

        /// <summary>
        /// Initializes a new instance of the <see cref="ATIOmega85"/> class
        /// </summary>
        /// <param name="ipAddress">IP address of the net box of the sensor</param>
        public ATIOmega85(string ipAddress)
        {
            this.udpClient = new UdpClient();
            this.ipAddress = ipAddress;
            this.IsActive = false;
            this.configurationObtained = false;

            this.udpClient.Connect(ipAddress, ATIPort);
            this.udpClient.DontFragment = true;

            this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), ATIPort);

            this.request = new byte[ATICommand.CommandLengthInByte];
            this.GetActiveSensorConfiguration();

            this.disposed = false;
        }

        /// <summary>
        /// Gets a value indicating whether the sensor is active
        /// </summary>
        public bool IsActive { get; private set; }
        
        /// <summary>
        /// Async call back to store the bytes that are received most recently
        /// </summary>
        /// <param name="ar">Async callback result</param>
        public void ReceiveCallback(IAsyncResult ar)
        {
            // lock access to the inner udp client
            // to deal with situation when the object is accessed after
            // it is already disposed from its main thread
            bool lockTaken = false;
            this.spinLock.Enter(ref lockTaken);

            if (!lockTaken)
            {
                throw new InvalidOperationException("Could not aquire the lock for receiving messages.");
            }

            if (true == this.disposed)
            {
                return;
            }

            this.mostRecentlyReceivedBytes = this.udpClient.EndReceive(ar, ref this.endPoint);

            // write the current message to the internal buffer
            while (0 != this.udpClient.Available)
            {
                this.mostRecentlyReceivedBytes = this.udpClient.Receive(ref this.endPoint);
            }

            // parse the current message
            // should be protected by the spin lock since the member variables for storing most recent force and torque information
            // can be accessed from another thread
            this.ParseOneMessage();
            
            // release the lock
            this.spinLock.Exit();

            // start receiving next message
            this.udpClient.BeginReceive(new AsyncCallback(this.ReceiveCallback), null);
        }

        /// <summary>
        /// Get the active configuration
        /// </summary>
        public void GetActiveSensorConfiguration()
        {
            // get configuration page in xml format
            string configurationAddress = string.Format("http://{0}/{1}", this.ipAddress, ATICommand.ConfigurationPageAddress);
            WebRequest webRequest = WebRequest.Create(configurationAddress);
            WebResponse webResp = webRequest.GetResponse();

            Stream dataStream = webResp.GetResponseStream();
            StreamReader streamReader = new StreamReader(dataStream);
            string responseFromServer = streamReader.ReadToEnd();

            // read relevant configuration data from the xml page
            using (XmlReader reader = XmlReader.Create(new StringReader(responseFromServer)))
            {
                if (reader.Name.Equals(ATIResponse.ActiveForceUnitNodeName) || reader.ReadToFollowing(ATIResponse.ActiveForceUnitNodeName))
                {
                    this.isForceMeasuredInNewton = (reader.ReadElementContentAsString().Equals(ATIResponse.ForceUnitNewtonXMLValue));
                }

                if (reader.Name.Equals(ATIResponse.ActiveTorqueUnitNodeName) || reader.ReadToFollowing(ATIResponse.ActiveTorqueUnitNodeName))
                {
                    this.isTorqueMeasuredInNewtonMeter = (reader.ReadElementContentAsString().Equals(ATIResponse.TorqueUnitNewtonMeterXMLValue));
                }

                if (reader.Name.Equals(ATIResponse.ActiveCountsPerForceReadingNodeName) || reader.ReadToFollowing(ATIResponse.ActiveCountsPerForceReadingNodeName))
                {
                    this.countPerForce = reader.ReadElementContentAsInt();
                }

                if (reader.Name.Equals(ATIResponse.ActiveCountsPerTorqueReadingNodeName) || reader.ReadToFollowing(ATIResponse.ActiveCountsPerTorqueReadingNodeName))
                {
                    this.countPerTorque = reader.ReadElementContentAsInt();
                }

                if (false == this.isForceMeasuredInNewton || false == this.isTorqueMeasuredInNewtonMeter)
                {
                    throw new InvalidOperationException("Please select force to be measured in Newton and torque to be measured in Newton-Meter.  To select a different configuration, visit the homepage of the web server on the Net Box (using the same IPV4 address as the sensor) and change the units accordingly.");
                }
            }

            this.configurationObtained = true;
        }

        /// <summary>
        /// Send a request to start streaming indefinitely
        /// </summary>
        public void StartStreaming()
        {
            if (false == this.configurationObtained)
            {
                throw new InvalidOperationException("Sensor configuration has not been successfully obtained. Streaming not started.  Call GetActiveSensorConfiguration() to obtain sensor configuration.");
            }

            byte[] header = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.CommandHeader));
            this.request[0] = header[0];
            this.request[1] = header[1];

            byte[] command = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.StartRealTimeStreaming));
            this.request[2] = command[0];
            this.request[3] = command[1];

            byte[] count = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.StreamingIndefinitely));
            this.request[4] = count[0];
            this.request[5] = count[1];
            this.request[6] = count[2];
            this.request[7] = count[3];

            // ask remote sensor to start sending force torque information
            this.udpClient.Send(this.request, ATICommand.CommandLengthInByte);

            // start listening
            this.udpClient.BeginReceive(new AsyncCallback(this.ReceiveCallback), null);

            this.IsActive = true;
        }

        /// <summary>
        /// Send a request to stop streaming
        /// </summary>
        public void StopStreaming()
        {
            byte[] header = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.CommandHeader));
            this.request[0] = header[0];
            this.request[1] = header[1];

            byte[] command = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.StopStreaming));
            this.request[2] = command[0];
            this.request[3] = command[1];

            byte[] count = BitConverter.GetBytes(IPAddress.HostToNetworkOrder(ATICommand.NotInUse));
            this.request[4] = count[0];
            this.request[5] = count[1];
            this.request[6] = count[2];
            this.request[7] = count[3];

            this.udpClient.Send(this.request, ATICommand.CommandLengthInByte);

            this.IsActive = false;
        }

        /// <summary>
        /// Get one message remote sensor
        /// </summary>
        /// <param name="force">Force result</param>
        /// <param name="torque">Torque result</param>
        public void GetCurrentForceTorque(out Vector3 force, out Vector3 torque)
        {
            // lock access to the inner udp client
            bool lockTaken = false;
            this.spinLock.Enter(ref lockTaken);

            if (!lockTaken)
            {
                throw new InvalidOperationException("Could not aquire the lock for disposing process.");
            }

            // copy out the most recent force torque information
            force = this.mostRecentForce;
            torque = this.mostRecentTorque;

            this.spinLock.Exit();
        }

        /// <summary>
        /// Release resources.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Parse one message
        /// </summary>
        private void ParseOneMessage()
        {
            byte[] data = this.mostRecentlyReceivedBytes;

            if (null == data)
            {
                TraceOut.Warning(sensorMonitorContext, "No data to parse");
                return;
            }

            if (data.Length != ATICommand.ResponseLengthInByte)
            {
                TraceOut.Warning(sensorMonitorContext, "Response message size ({0}) is not as expected ({1})", data.Length, ATICommand.ResponseLengthInByte);
                return;
            }

            // data[0-3] contains RDT sequence, data[4-7] contains FT sequence, data[8-11] contains status information
            // they are not used at the moment and thus not parsed here.

            // Force
            this.mostRecentForce.X = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 12))) / ((double)this.countPerForce);
            this.mostRecentForce.Y = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 16))) / ((double)this.countPerForce);
            this.mostRecentForce.Z = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 20))) / ((double)this.countPerForce);
            this.mostRecentTorque.X = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 24))) / ((double)this.countPerTorque);
            this.mostRecentTorque.Y = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 28))) / ((double)this.countPerTorque);
            this.mostRecentTorque.Z = ((double)IPAddress.NetworkToHostOrder(BitConverter.ToInt32(data, 32))) / ((double)this.countPerTorque);
        }

        /// <summary>
        /// Dispose of resources
        /// </summary>
        /// <param name="disposing">True if Dispose() was called.</param>
        private void Dispose(bool disposing)
        {
            if (!this.disposed)
            {
                // lock access to the inner udp client
                bool lockTaken = false;
                this.spinLock.Enter(ref lockTaken);

                if (!lockTaken)
                {
                    throw new InvalidOperationException("Could not aquire the lock for disposing process.");
                }

                if (disposing)
                {
                    // dispose of managed resources
                    if (true == this.IsActive)
                    {
                        this.StopStreaming();
                        this.IsActive = false;
                    }
                }

                // dispose of unmanaged resources
                if (null != this.udpClient)
                {
                    this.udpClient.Close();
                }

                this.disposed = true;

                // release the lock
                this.spinLock.Exit();
            }
        }
    }
}
